//Test1 per la sparkfun 6 DOF
//primo test
//lettura supercazzona, si può migliorare sensibilmente prendendo parallelamente 
//il valore dei registri di input 
#include <stdio.h>
unsigned long timer=0; //timer
unsigned long  dt;     //delta t - tempo esecuzione del main loop (robe sull'acqusition rate)
char buf[128];
int value[6] = {0,0,0,0,0,0};  // dati {G y, G x, G z, A x, A y, A x} con G il gyroscopio e A l'accelerometro
struct imu{
  int ax,ay,az,gz,gx,gy;

}values;
void setup()
{

  Serial.begin(115200);
  delay(20);  
  analogReference(EXTERNAL);     //Setto il valore di riferimento analogico esterno (arduino-DOF)
				 //In pratica il valore di riferimento sarà il voltaggio applicato sul pin AREF. 
				 //Consigliano una resistenza da 5k sul pin (da notare il conseguente effetto di voltage divider con la resistenza interna da 32k)
  delay(200);
  DDRC = B00000000;              //setta tutt gli analog pin a input  TODO: tutte le altre init
  //timer=millis();
  //Serial.println("t[ms] \t gx \t gy \t gz \t ax \t ay \t az ");
}

void loop()
{
 // dt = millis() - timer;
  //timer=millis();       
 // Serial.print(dt);      
 // Serial.print ("\t"); 
  read_DOF();            //leggo la roba
  print_val();
  delay(20);
}

void read_DOF() //read sensor data
{
  int * pvalues = (int*)(&values);
  for (int i=0; i<6; i++) {
    pvalues[i] = (analogRead(i)) ; //qui possiamo ciacciare i valore letti (trucchetti con offset calcolati o filtri vari)
                    	       //questa lettura va necessariamente ottimizzata per fare i fighi 
			       //l'alanoloRead si prende ~100 microsecondi. quindi 10000 letture al secondo max.
 //la cosa più seria da fare sarebbe utilzzare un convertitore veloce esterno multicanale (uno per pin della DOF) e beccarsi con intterupt le letture
 //esistono  A/D su I2C abbastanza economici
   }
}


void print_val() //print data
{ sprintf(buf,"%d\t%d\t%d\t%d\t%d\t%d\t",values.gx,values.gy,values.gz,values.ax,values.ay,values.az);
  Serial.print(buf);
  Serial.println("");
}
